#include "WorkpieceE452.h"
#include "../mesurementinterface.h"
#include "MeasurE452AlgBase.h"
#include "../FitAdjust.hpp"

#include <QThread>
#include <unistd.h>

WorkpieceE452::WorkpieceE452():
    m_threadStopFlag(false),
    m_isSetPathFlag(false),
    m_initThreadFinished_(false)/*,
    m_isDataCorrect(false)*/
{
    m_workpieceBuff.SetSize(MAX_WORKPIECE_BUFFER_NUM);
    if(!m_initThreadFinished_) {
        InitThread();
    }
}

WorkpieceE452::~WorkpieceE452()
{
    ExitThread();
}

void WorkpieceE452::CacheData(const cv::Mat &image         ,
                              const QPoint &datumOffset    ,
                              int workpieceId              ,
                              int pictureId                ,
                              int channelId                ,
                              int fixtureId)
{
    if(!m_workpieceMap.contains(workpieceId))
    {
        WorkpieceInfo workpieceInfo;
        workpieceInfo.workpieceId  = workpieceId            ;
        workpieceInfo.channelId    = channelId              ;
        workpieceInfo.fixtureId    = fixtureId              ;
        m_workpieceMap.insert(workpieceId, workpieceInfo)   ;
    }
    switch (pictureId) {
    case PICTURE_ID_36:
    case PICTURE_ID_13:
    case PICTURE_ID_37:
        m_workpieceMap[workpieceId].imageMap[pictureId]            = image;
        m_workpieceMap[workpieceId].imageDatumOffsetMap[pictureId] = datumOffset;
        m_workpieceMap[workpieceId].imageNum++;
        m_workpieceMap[workpieceId].curPictureId = pictureId  ;
        break;
    default:
        break;
    }
    // if finished input to queue
    //判断当前图像是否可以计算，如果可以计算，发送信号进行计算
    if(m_workpieceMap[workpieceId].IsPictureUseable(WORKPIECE_TYPE_E452, pictureId)) {
        m_workpieceBuff.SetData(m_workpieceMap[workpieceId]);

        if(m_workpieceMap[workpieceId].IsWorkpieceUseable(WORKPIECE_TYPE_E452)) {
            m_workpieceMap.remove(workpieceId);
        }
        //
        // 此处默认工件号由小到大进行排序，如果当前工件号和最小工件号相差MAX_WAIT_WORKPIECE_NUM个，
        // 说明最小工件号的工件图像已经丢失，
        // 需要删除该工件信息
        QMap<int, WorkpieceInfo>::iterator iter = m_workpieceMap.begin();
        int minWorkpieceId = iter.key();
        if(workpieceId - minWorkpieceId > MAX_WAIT_WORKPIECE_NUM) {
            m_workpieceMap.remove(minWorkpieceId);
            qDebug() << "workpiece : " << workpieceId << "lost";
        }
    }
}

bool WorkpieceE452::SetTemplatePath(const std::string &path)
{
    MeasurE452AlgBase::SetTemplatePath(path);
    m_configPath = path;
    m_isSetPathFlag = true;
    return true;
}

bool WorkpieceE452::SetStandardVal(const std::map<string, MeasureItemStandardVal> &standardValMap)
{
    std::map<std::string, float> standardValStrMap;
    for(const auto &item: standardValMap) {
        std::string keyStr = item.first;
        standardValStrMap.insert(make_pair(keyStr, item.second.standardVal));
    }
    MeasurE452AlgBase::SetStandardVal(standardValStrMap);
    return true;
}

void WorkpieceE452::ProcessData()
{
    qDebug() << "start to E452 measure workpiece";
    while(!m_isSetPathFlag) {
        sleep(0.1);
    }

    MeasurE452AlgBackDatum                  op0(MAX_THREAD_NUM_E452);
    MeasurE452AlgBackL1toL8Circle           op1(MAX_THREAD_NUM_E452);
    MeasurE452AlgBackR1toR4Circle           op2(MAX_THREAD_NUM_E452);
    MeasurE452AlgBackD1ToD8Circle           op3(MAX_THREAD_NUM_E452);
    MeasurE452AlgBackSS1SS2Circle           op4(MAX_THREAD_NUM_E452);
    MeasurE452AlgBackS1S2S4S5HighS3Circle   op5(MAX_THREAD_NUM_E452);
    MeasurE452AlgFrontSE1toSE9Edge          op6(MAX_THREAD_NUM_E452);
    MeasurE452AlgHighSs3ToSs7Circle         op7(MAX_THREAD_NUM_E452);

    int id = -1;

    while(!m_threadStopFlag)
    {
        WorkpieceInfo workpieceInfo;
        if(!m_workpieceBuff.GetData(workpieceInfo)) {
            qDebug() << "current thread exit" << QThread::currentThreadId();
            return;
        }

        // process data
        cv::Mat imgBack  = workpieceInfo.imageMap[PICTURE_ID_36].clone();
        cv::Mat imgFront = workpieceInfo.imageMap[PICTURE_ID_13].clone();
        cv::Mat imgHigh  = workpieceInfo.imageMap[PICTURE_ID_37].clone();

        int workpieceId = workpieceInfo.workpieceId;
        int channelId   = workpieceInfo.channelId  ;
        int fixtureId   = workpieceInfo.fixtureId  ;

#ifdef MEASURE_TIME
        auto RecordStartTime = [&](int workpieceId, int pictureId) {
            QString key = QString("%1-%2").arg(workpieceId).arg(pictureId);
            QTime *startTime = new QTime();
            g_measurementCostTime[key] = startTime;
            startTime->start();
        };

        RecordStartTime(workpieceId, PICTURE_ID_13);
        RecordStartTime(workpieceId, PICTURE_ID_36);
        RecordStartTime(workpieceId, PICTURE_ID_37);
#endif

        id++;
        id = id%MAX_THREAD_NUM_E452;

        std::map<std::string, float> measRes  ;
        std::map<std::string, float> measRes0 ;
        std::map<std::string, float> measRes1 ;
        std::map<std::string, float> measRes2 ;
        std::map<std::string, float> measRes3 ;
        std::map<std::string, float> measRes4 ;
        std::map<std::string, float> measRes5 ;
        std::map<std::string, float> measRes6 ;
        std::map<std::string, float> measRes7 ;

        std::thread t0(op0, std::ref(imgBack), std::ref(id));
        std::thread t1(op1, std::ref(imgBack), std::ref(id), std::ref(measRes1), fixtureId);
        std::thread t2(op2, std::ref(imgBack), std::ref(id), std::ref(measRes2), fixtureId);
        std::thread t3(op3, std::ref(imgBack), std::ref(id), std::ref(measRes3), fixtureId);
        std::thread t4(op4, std::ref(imgBack), std::ref(id), std::ref(measRes4), fixtureId);
        std::thread t5(op5, std::ref(imgBack), std::ref(imgHigh), std::ref(id),
                       std::ref(measRes5), fixtureId);
        std::thread t6(op6, std::ref(imgFront),std::ref(id), std::ref(measRes6),
                       fixtureId);
        std::thread t7(op7, std::ref(imgHigh), std::ref(imgBack),
                       std::ref(id), std::ref(measRes7),
                       fixtureId);

        t0.join();
        t1.join();
        t2.join();
        t3.join();
        t4.join();
        t5.join();
        t6.join();
        t7.join();

        auto MergeResult = [&](std::map<string, float> &measureInfo,
                std::map<string, float> measureInfoSub) {
            for(const auto &item : measureInfoSub) {
                measureInfo.insert(make_pair(item.first, item.second));
            }
        };

        QMap<QString, QString> error;
        std::map<std::string, float> measResBack;
        MergeResult(measResBack, measRes1) ;
        MergeResult(measResBack, measRes2) ;
        MergeResult(measResBack, measRes3) ;
        MergeResult(measResBack, measRes4) ;
        MergeResult(measResBack, measRes5) ;

        CorrectThreeD(measResBack, fixtureId);

        emit MesurementResultResponse(workpieceId,
                                      PICTURE_ID_36,
                                      channelId,
                                      fixtureId,
                                      measResBack, error);

        qDebug() << "workpieceId: "
                 << workpieceId
                 << "pictureId:"
                 << PICTURE_ID_13
                 << "channelId: "
                 << channelId;

        std::map<std::string, float> measResFront;
        MergeResult(measResFront, measRes6);
        CorrectThreeD(measResFront, fixtureId);

        emit MesurementResultResponse(workpieceId, PICTURE_ID_13, channelId,
                                      fixtureId, measResFront, error);

        qDebug() << "workpieceId: "
                 << workpieceId
                 << "pictureId:"
                 << PICTURE_ID_13
                 << "channelId: "
                 << channelId;

        std::map<std::string, float>   measureInfoHigh;
        MergeResult(measureInfoHigh, measRes7);
        CorrectThreeD(measureInfoHigh, fixtureId);
        emit MesurementResultResponse(workpieceId, PICTURE_ID_37,
                                      channelId,
                                      fixtureId,
                                      measureInfoHigh,
                                      error);

        qDebug() << "workpieceId: "
                 << workpieceId
                 << "pictureId:"
                 << PICTURE_ID_37
                 << "channelId: "
                 << channelId;
        MeasurE452AlgBase::clearAllflag();
    }
    qDebug() << "end to E452 measure workpiece and quit thread";
}

bool WorkpieceE452::GetDatumPixelDistance(const QImage &img, int fixtureId, QMap<QString, float> &pixelDistance)
{
    return false;
}

void WorkpieceE452::InitThread()
{
    for(int i = 0; i < MAX_THREAD_NUM_MEASURE; i++) {
        qDebug() << "start thread";
        m_threadPool.emplace_back(std::thread(&WorkpieceE452::ProcessData, this));
    }
    qDebug() << "start thread success";
    m_initThreadFinished_ = true;
}

void WorkpieceE452::ExitThread()
{
    if(m_initThreadFinished_) {
        if(!m_threadStopFlag) {
            m_threadStopFlag = true;
        }
        qDebug() << "start to exit thread";

        int loopCount = 0;
        while(!m_workpieceBuff.Empty() && loopCount < 3) {
            sleep(1);
            loopCount++;
        }

        // 调用该语句释放条件变量的等待条件
        m_workpieceBuff.Exit();

        for(size_t i = 0; i < m_threadPool.size(); ++i) {
            m_threadPool[i].join();
        }

        qDebug() << "measurement thread exit safely!";
    }
}

void WorkpieceE452::CorrectThreeD(std::map<string, float> &measRes, int fixtureId)
{
    if(MeasurementInterface::m_isDataCorrect) {
        QMap<QString, MeasurementInterface::MeasureInfo> qMapInfo;
        for (const auto &item : measRes)
        {
            QString &&name = QString::fromStdString(item.first);
            float dist = item.second;

            auto strlist = name.split('_');
            QString itemName = strlist[0];
            if (strlist.back() == "pixel")
                qMapInfo[itemName].pixel_distance = measRes[item.first];
            else if(strlist.back() == "real"){
                qMapInfo[itemName].real_distance = measRes[item.first];
            } else {
                // position not process
            }
        }

        QMap<QString, MeasurementInterface::MeasureInfo>::ConstIterator iter = qMapInfo.begin();
        vector<AdjustDataInfo> d;
        for(iter; iter != qMapInfo.end(); iter++) {
            std::string keyTempStr = iter.key().toStdString();
            multimap<double, double> datas;
            datas.insert(make_pair(iter.value().pixel_distance, iter.value().real_distance));
            AdjustDataInfo adjustDataInfo = {keyTempStr, datas};
            d.emplace_back(adjustDataInfo);
        }

        vector<AdjustDataInfo>* ptr = new vector<AdjustDataInfo>(d);

        std::string path;
        if(0 == fixtureId) {
            path = (MeasurE452AlgBase::m_dir_distance_fitting_model + "/test7.xml");
        } else {
            path = (MeasurE452AlgBase::m_dir_distance_fitting_model + "/test8.xml");
        }

        PixelOutliersClassify adjust(path);
        adjust.adjustOutlierData(ptr);
        auto change_data = *ptr;
        for(auto& item: change_data) {
            for(auto& data: item.item_data) {
                std::string faiItemName = item.item_name;
                qDebug()<< "pixel data:" << data.first << " ";
                qDebug()<< "prediction change : " << data.second << endl;
                std::string faiItemNameReal = faiItemName + "_real";
                measRes[faiItemNameReal] = data.second;
            }
        }

        auto PositionCorrectFun = [&](std::map<string, float> &measRes,
                std::string posStr,
                std::string xStr,
                std::string yStr)
        {
            if(measRes.count(posStr) > 0) {
                measRes[posStr] = CalPosition(measRes, xStr, yStr);
            }
        };

        PositionCorrectFun(measRes, "FAI19-R-position", "FAI19-R-X-distance_real", "FAI19-R-Y-distance_real");
        PositionCorrectFun(measRes, "FAI22-L-position", "FAI22-L-X-distance_real", "FAI22-L-Y-distance_real");
        PositionCorrectFun(measRes, "FAI25-D-position", "FAI25-D-X-distance_real", "FAI25-D-Y-distance_real");
        PositionCorrectFun(measRes, "FAI154-SS1SS4-position", "FAI154-SS1SS4-X-distance_real", "FAI154-SS1SS4-Y-distance_real");
        PositionCorrectFun(measRes, "FAI155-SS6SS4-position", "FAI155-SS6SS4-X-distance_real", "FAI155-SS6SS4-Y-distance_real");
    }
}

float WorkpieceE452::CalPosition(std::map<std::string, float> &measRes,
                                 std::string strX, std::string strY)
{
    if(measRes.empty()) {
        qDebug() << "gray algorithm: E452 caculate position error";
        return -1;
    }
    int endIndexX = strX.find("_real");
    int endIndexY = strY.find("_real");
    std::string standardXStr = strX.substr(0, endIndexX);
    std::string standardYStr = strY.substr(0, endIndexY);
    return 2*sqrt(((measRes[strX] - MeasurE452AlgBase::m_standardValMap[standardXStr]) * (measRes[strX] - MeasurE452AlgBase::m_standardValMap[standardXStr]))
                  + ((measRes[strY] - MeasurE452AlgBase::m_standardValMap[standardYStr]) * (measRes[strY] - MeasurE452AlgBase::m_standardValMap[standardYStr])));
}
